package bojan.robots.motion;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.Set;

import sim.field.continuous.Continuous2D;
import sim.util.Double2D;
import sim.util.Int2D;
import src.bojan.TerrainMap;

public class Driver {

	public static final int SPACE_LIMIT = 200;
	// ObstacleManager obstacleManager;
	Continuous2D grid;
	Motion motion;
	private TerrainMap map;
	private DistanceEvaluator distanceEvaluator;

	public Driver(Continuous2D grid, Motion motion, TerrainMap map,
			DistanceEvaluator distanceEvaluator) {
		super();
		this.grid = grid;
		this.motion = motion;
		this.map = map;
		this.distanceEvaluator = distanceEvaluator;
	}

	public Set<Double2D> alternatives(Double2D p, Int2D boundary) {
		// TODO Maybe I should limit size of step to 1.0
//		System.out.println(boundary);
		if (boundary.x != 0) {
			Set<Double2D> n = new HashSet<Double2D>();
			n.add(new Double2D(position(p.x, boundary.x),
					p.y + 1.0));
			n.add(new Double2D(position(p.x, boundary.x),
					p.y - 1.0));
			return n;
		} else if (boundary.y != 0) {
			Set<Double2D> n = new HashSet<Double2D>();
			n.add(new Double2D(p.x + 1.0, position(p.y, boundary.y)));
			n.add(new Double2D(p.x - 1.0, position(p.y, boundary.y)));
			return n;
		}
		return null;
	}

	

	private double position(double p, double boundary) {
		if (boundary > p ) {
			return boundary - 0.5;
		}
		return boundary + 1.5;
	}

	public double calcCoordinate(double p, double np) {
		if (Math.abs(p - np) < 1) {
			return np;
		} else if (Math.abs(p - np) == 1) {
			return ((np - p) / Math.abs(np - p)) * SPACE_LIMIT;
		} else {
			throw new RuntimeException(
					"The difference between coords shouldn't be more than 1.");
		}

	}

	public Double2D newGoal(Double2D p, Double2D np) {
		//

		return new Double2D(calcCoordinate(p.x, np.x),
				calcCoordinate(p.y, np.y));
	}

	public List<Double2D> potentialObstacles(Double2D p, Double2D m) {
		Double2D npx = new Double2D(p.x + new Double2D(m.x, 0).resize(1.0).x,
				p.y);
		Double2D npy = new Double2D(p.x, p.y
				+ new Double2D(0, m.y).resize(1.0).y);

		// Double2D closest = obstacleManager.closestObstacle(p, npx, npy);
		// if (npx.equals(closest)) {
		// return new Int2D(grid.discretize(closest).x, 0);
		// } else {p
		// return new Int2D(0, grid.discretize(closest).y);
		// }
		ArrayList<Double2D> a = new ArrayList<Double2D>();
		a.add(npy); // TODO Remove point in same square as the calculated dots
		a.add(npx);
		return a;
	}

	// public Double2D nextPosition(Double2D a_position, Double2D m, Double2D
	// goal) {
	//
	// // Int2D bound = boundary(a_position, m);
	// List<Double2D> bn = boundary(a_position, m);
	// Double2D c = closest(bn, goal);
	//
	// Int2D obstacle = getBounds(c, a_position);
	// new DistanceEvaluator(alternative(a_position, obstacle.x, obstacle.y),
	// goal);
	// Double2D a = nextAlongObstacle();
	// if (a != null) {
	// return a;
	// }
	// return null;
	// }

	public Int2D getBounds(Double2D a_position, Double2D c) {
		// TODO Auto-generated method stub
		return new Int2D(a_position.x == c.x ? 0 : grid.discretize(c).x,
				a_position.y == c.y ? 0 : grid.discretize(c).y);
	}

	// public Double2D nextAlongObstacle( Collection<Double2D> k, Double2D goal)
	// {
	//
	// Double2D ;
	// while ((next = distanceEvaluator.nextMove()) != null) {
	// if (motionValidator.valid(next)) {
	// return next;
	// }
	// }
	//
	// closest(k, goal);
	// if (!obstacleManager.isObstacle(next)
	// ) {
	//
	// return next;
	// }
	// k.remove(next);
	// next = k.iterator().next();
	// if (!backToSamePosition(m, step(a_position, next))) {
	// return next;
	// }
	//
	// return null;
	// }

	public boolean backToSamePosition(Double2D m, Double2D step) {

		// TODO this condition should be revisited.
		return m.add(step).length() < 0.1;
	}

	public Double2D step(Double2D a_position, Double2D c) {
		// TODO 1.0 variable

		System.out.println(c);
		// if (Math.abs(c.x - a_position.x) == 1) {
		// c = new Double2D(a_position.x, c.y);
		// }
		return c.subtract(a_position).resize(1.0);
	}

	public Set<Double2D> nextPosition(Double2D position, Double2D goal) {

		Double2D newPosition = motion.nextPosition(position, goal);
		if (!map.isObstacle(newPosition)) {
			Set<Double2D> a = new HashSet<Double2D>();
			a.add(newPosition);
			return a;
		}
		// boundary(p, m)
		return moveAlongBoundaries(position, goal);
	}

	public Set<Double2D> moveAlongBoundaries(Double2D position, Double2D goal) {
		Double2D m = motion.step(position, goal);
		Collection<Double2D> boundaries = obstacles(position, m);
		Set<Double2D> alternatives = new HashSet<Double2D>();
		for (Double2D boundary : boundaries) {
			alternatives.addAll(alternativesAlongBoundary(position, boundary));
		}		
		return alternatives;
	}

	

	public Set<Double2D> alternativesAlongBoundary(Double2D position,
			Double2D boundary) {
		return map.nonObstacles(alternatives(position,
				getBounds(position, boundary)));
	}

	public Collection<Double2D> obstacles(Double2D position, Double2D m) {
		return map.obstacles(potentialObstacles(position, m));
	}

	// public Double2D moveAlongABoundary(Double2D position, Double2D boundary,
	// Double2D goal) {
	//
	// Set<Double2D> alternatives = alternatives(position,
	// getBounds(position, boundary));
	// Double2D closest = distanceEvaluator.closest(alternatives, goal);
	// if (map.isFree(closest)) {
	// return closest;
	// }
	// return null;
	//
	// }

	// public Double2D nextPosition(Double2D a_position, Double2D m, Double2D
	// goal) {
	//
	// // Int2D bound = boundary(a_position, m);
	// List<Double2D> bn = boundary(a_position, m);
	// Double2D c = closest(bn, goal);
	//
	// Int2D obstacle = getBounds(c, a_position);
	// new DistanceEvaluator(alternative(a_position, obstacle.x, obstacle.y),
	// goal);
	// Double2D a = nextAlongObstacle();
	// if (a != null) {
	// return a;
	// }
	// return null;
	// }

}
